/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package nx2.utils;
import javaclient2.*;
import javaclient2.structures.blobfinder.*;
import javaclient2.structures.planner.*;
import javaclient2.structures.graphics2d.*;
import javaclient2.structures.*;
import java.util.*;
/**
 *
 * @author alejo
 */
public class Graphics2DHelper {


    private Graphics2DInterface graph;

    public Graphics2DHelper(Graphics2DInterface graph)
    {
        this.graph = graph;
    }

    public void drawRobotPath(PlannerInterface plan)
    {
        //color para las lineas que definen la trayectoria del robot
        PlayerColor colorLines = new PlayerColor();
        colorLines.setBlue(255);
        colorLines.setGreen(0);
        colorLines.setRed(50);

        //color para la posicion del robot
        PlayerColor colorRobot = new PlayerColor();
        colorRobot.setBlue(0);
        colorRobot.setGreen(0);
        colorRobot.setRed(255);

                //color para la posicion de la meta
        PlayerColor colorGoal = new PlayerColor();
        colorGoal.setBlue(0);
        colorGoal.setGreen(255);
        colorGoal.setRed(0);


        //informacion sobre los puntos calculados
        plan.getWaypoints();
        PlayerPlannerWaypointsReq pointData = plan.getWaypointData();

        PlayerPose[] points = pointData.getWaypoints();
        int numPoints = pointData.getWaypoints_count();

        //si efectivamente existen puntos, genere el array de puntos y grafique
        if(numPoints>0)
        {
            PlayerPoint2d[] pointPathArr = new PlayerPoint2d[numPoints];

            PlayerPose myGoal = plan.getData().getGoal();

            //el primer punto es la posicion
            PlayerPose myPos = plan.getData().getPos();

            pointPathArr[0] = new PlayerPoint2d();
            pointPathArr[0].setPx(myPos.getPx());
            pointPathArr[0].setPy(myPos.getPy());


            for(int i = 0;i<numPoints-1;i++)
            {
                pointPathArr[i+1] = new PlayerPoint2d();
                pointPathArr[i+1].setPx(points[i+1].getPx());
                pointPathArr[i+1].setPy(points[i+1].getPy());
            }

            PlayerGraphics2dCmdPolyline lines = new PlayerGraphics2dCmdPolyline();
            lines.setPoints(pointPathArr);
            lines.setCount(numPoints);

            lines.setColor(colorLines);
            graph.drawPolyline(lines);

            //ahora dibuje el robot

            //puntos
            PlayerPoint2d[] pointRobotArr = new PlayerPoint2d[4];

            pointRobotArr[0] = new PlayerPoint2d();
            pointRobotArr[1] = new PlayerPoint2d();
            pointRobotArr[2] = new PlayerPoint2d();
            pointRobotArr[3] = new PlayerPoint2d();

            pointRobotArr[0].setPx(myPos.getPx()-0.1f);
            pointRobotArr[0].setPy(myPos.getPy()-0.1f);
            pointRobotArr[1].setPx(myPos.getPx()+0.1f);
            pointRobotArr[1].setPy(myPos.getPy()-0.1f);
            pointRobotArr[2].setPx(myPos.getPx()+0.1f);
            pointRobotArr[2].setPy(myPos.getPy()+0.1f);
            pointRobotArr[3].setPx(myPos.getPx()-0.1f);
            pointRobotArr[3].setPy(myPos.getPy()+0.1f);

            //poligono que representa al robot
            PlayerGraphics2dCmdPolygon polyRobot = new PlayerGraphics2dCmdPolygon();

            polyRobot.setColor(colorRobot);
            polyRobot.setFill_color(colorRobot);
            polyRobot.setFilled(0);
            polyRobot.setCount(4);
            polyRobot.setPoints(pointRobotArr);
            graph.drawPolygon(polyRobot);

            //finalmente dibuje la meta

            //puntos
            PlayerPoint2d[] pointGoaltArr = new PlayerPoint2d[4];

            pointGoaltArr[0] = new PlayerPoint2d();
            pointGoaltArr[1] = new PlayerPoint2d();
            pointGoaltArr[2] = new PlayerPoint2d();
            pointGoaltArr[3] = new PlayerPoint2d();

            pointGoaltArr[0].setPx(myGoal.getPx()-0.1f);
            pointGoaltArr[0].setPy(myGoal.getPy()-0.1f);
            pointGoaltArr[1].setPx(myGoal.getPx()+0.1f);
            pointGoaltArr[1].setPy(myGoal.getPy()-0.1f);
            pointGoaltArr[2].setPx(myGoal.getPx()+0.1f);
            pointGoaltArr[2].setPy(myGoal.getPy()+0.1f);
            pointGoaltArr[3].setPx(myGoal.getPx()-0.1f);
            pointGoaltArr[3].setPy(myGoal.getPy()+0.1f);

            //poligono que representa al robot
            PlayerGraphics2dCmdPolygon polyGoal = new PlayerGraphics2dCmdPolygon();

            polyGoal.setColor(colorGoal);
            polyGoal.setFill_color(colorGoal);
            polyGoal.setFilled(1);
            polyGoal.setCount(4);
            polyGoal.setPoints(pointGoaltArr);
            graph.drawPolygon(polyGoal);
        }

    }

    /**
     * Useful for cameras to show the coverage of the image.
     * @param poss an array of Position2DInterfaces of the cameras
     * @param camCoverage Field of View (fov) of the camera in [rad].
     * @param camMaxDist Max. distance where the camera can sense intruders.
     */
    public void drawCamCoverage(Position2DInterface[] poss, double camCoverage, double camMaxDist)
    {

        //color para el poligono que define cobertura de la camara
        PlayerColor color = new PlayerColor();
        color.setBlue(255);
        color.setGreen(0);
        color.setRed(255);
        color.setAlpha(255);

        for(int j=0;j<poss.length;j++)
        {
            //posicion de la camara en el entorno
            Position2DInterface pos = poss[j];
            PlayerPose iPos = pos.getData().getPos();

            //creacion de puntos del poligono que representa cobertura de cam
            PlayerPoint2d point1 = new PlayerPoint2d();
            PlayerPoint2d point2 = new PlayerPoint2d();
            PlayerPoint2d point3 = new PlayerPoint2d();
            PlayerPoint2d point4 = new PlayerPoint2d();
            //array de puntos
            PlayerPoint2d[] pointArray = {point1, point2, point3, point4};

            //primer punto es la camara
            point1.setPx(iPos.getPx());
            point1.setPy(iPos.getPy());

            //segundo punto
            double x = Math.cos(camCoverage)*camMaxDist;
            double y = Math.sin(camCoverage)*camMaxDist;
            Vector2D secPoint = new Vector2D((float)x,(float)y,Vector2D.MODE_XY);
            secPoint.setAngle(secPoint.getAngle()+ iPos.getPa());
            secPoint = secPoint.add(new Vector2D(iPos));
            point2.setPx(secPoint.getPx());
            point2.setPy(secPoint.getPy());

            //tercer punto
            x = camMaxDist;
            y = 0;
            Vector2D fourthPoint = new Vector2D((float)x,(float)y,Vector2D.MODE_XY);
            fourthPoint.setAngle(fourthPoint.getAngle()+ iPos.getPa());
            fourthPoint = fourthPoint.add(new Vector2D(iPos));
            point3.setPx(fourthPoint.getPx());
            point3.setPy(fourthPoint.getPy());

            //cuarto punto
            x = Math.cos(camCoverage)*camMaxDist;
            y = Math.sin(-camCoverage)*camMaxDist;
            Vector2D thirdPoint = new Vector2D((float)x,(float)y,Vector2D.MODE_XY);
            thirdPoint.setAngle(thirdPoint.getAngle()+ iPos.getPa());
            thirdPoint = thirdPoint.add(new Vector2D(iPos));
            point4.setPx(thirdPoint.getPx());
            point4.setPy(thirdPoint.getPy());

            PlayerGraphics2dCmdPolygon polygon = new PlayerGraphics2dCmdPolygon();

            polygon.setColor(color);
            polygon.setFill_color(color);
            polygon.setFilled(0);
            polygon.setCount(4);
            polygon.setPoints(pointArray);

            graph.drawPolygon(polygon);
        }
    }

    /**
     * Draw intruders in the field.
     * @param intr An array of PlayerPoses of the intruders
     * @param intruderSize The size of the intruders
     */
    public void drawIntruders(PlayerPose[] intr, float intruderSize)
    {
        for(int i=0; i<intr.length;i++)
        {
            //color para los intrusos
            PlayerColor color = new PlayerColor();
            color.setBlue(0);
            color.setGreen(0);
            color.setRed(255);
            color.setAlpha(0);

            //el i-esimo intruso
            PlayerPose aIntr = intr[i];
            float x = aIntr.getPx();
            float y = aIntr.getPy();

            //creacion de puntos del poligono que representa la intruso
            PlayerPoint2d point1 = new PlayerPoint2d();
            PlayerPoint2d point2 = new PlayerPoint2d();
            PlayerPoint2d point3 = new PlayerPoint2d();
            PlayerPoint2d point4 = new PlayerPoint2d();

            PlayerPoint2d[] pointArray = {point1, point2, point3, point4};

            point1.setPx(x-intruderSize/2);
            point1.setPy(y-intruderSize/2);
            point2.setPx(x+intruderSize/2);
            point2.setPy(y-intruderSize/2);
            point3.setPx(x+intruderSize/2);
            point3.setPy(y+intruderSize/2);
            point4.setPx(x-intruderSize/2);
            point4.setPy(y+intruderSize/2);


            PlayerGraphics2dCmdPolygon polygon = new PlayerGraphics2dCmdPolygon();

            polygon.setColor(color);
            polygon.setFill_color(color);
            polygon.setFilled(1);
            polygon.setCount(4);
            polygon.setPoints(pointArray);

            graph.drawPolygon(polygon);



        }
    }
}
